#include <util/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include "sonar.h"
#include "os.h"

#define SONAR_DDR               DDRC						    /** data direction register of port C **/
#define SONAR_PORT				PORTC

#define SONAR_PORTNUM_ECHO		PORTC7
#define SONAR_ECHO_MASK   		(_BV(7))					    /** set Bit 7 **/

#define SONAR_PORTNUM_PULSE		PORTC6
#define SONAR_PULSE_MASK   		(_BV(6))					    /** set Bit 6 **/

#define SET_RISING_EDGE()	(TCCR3B |= _BV(ICES3)) 				/** set input capture to detect rising edge **/
#define SET_FALLING_EDGE()	(TCCR3B &= ~(_BV(ICES3)))			/** set input capture to detect falling edge **/
#define IS_RISING_EDGE()	(TCCR3B & _BV(ICES3))				/** check if rising edge on input capture set **/
#define IS_FALLING_EDGE()	~(TCCR3B & _BV(ICES3))				/** check if falling edge on input capture set **/

#define SET_IC_ENABLE()		(TIMSK3 |= _BV(ICIE3))				/**	set input capture interrupt enable **/
#define CLEAR_IC_FLAG()		(TIFR3 |= _BV(ICF3))				/** clear input capture flag **/

//#define ENABLE_ECHO()		(SONAR_PORT |= SONAR_ECHO_MASK)		/**	enable input capture on Port C, pin 7 **/
//#define DISABLE_ECHO()		(SONAR_PORT &= ~(SONAR_ECHO_MASK))	/** disable input capture on Port C, pin 7 **/
//#define ENABLE_PULSE()		(SONAR_PORT |= SONAR_PULSE_MASK)	/** enable output capture on Port C, pin 6 **/
//#define DISABLE_PULSE()		(SONAR_PORT &= ~(SONAR_PULSE_MASK))	/** disable output capture on Port C, pin 6 **/

static volatile uint16_t start_fall;
static volatile uint16_t readings[5] = {0,0,0,0,0};
static volatile int readidx = 0;
static volatile int done = 0;


void sonar_init(){
	TCCR3B &= ~(_BV(CS32) | _BV(CS30));	/**	set clock source to prescale by 8 **/
	TCCR3B |= _BV(CS31);				/**	set clock source to prescale by 8 **/
	TCCR3B |= _BV(ICNC3);				/**	enable noise cancelling **/

	/* Get first reading for each sonar */
	int idx;
	for(idx = 0; idx < 5; idx++){
		sonar_pulse(idx);
		_delay_ms(100); /* Sonar takes twice as long for first reading */
	}
}

uint8_t read_distance(int idx){
	return (readings[idx]/US_PER_INCH);	/** pulse width = 147 us/inch **/
}

int sonar_done(){
	return done;
}

void sonar_pulse(int idx){
	readidx = idx;
	readings[readidx] = 0xff;
	PORTC &= ~(0x1f);
	SET_RISING_EDGE();
	CLEAR_IC_FLAG();
	PORTC |= 1 << readidx;
}

/**
*
* ISR function
*
*/
ISR(TIMER3_CAPT_vect){
	PORTC &= ~(0x1f);
	if(IS_RISING_EDGE()){
		start_fall = ICR3;						/** reference point for start of pulse **/
		SET_FALLING_EDGE();
		CLEAR_IC_FLAG();
	}else{
		/** ICR3 = TCNT3 when interrupt detected **/
		PORTD ^= _BV(PORTD7);
		uint16_t reading = (ICR3 - start_fall);
#if 0
		readings[readidx] /= 2;
		readings[readidx] += reading / 2;
#else
		readings[readidx] = reading;
#endif
		done = 1;
	}
}
